import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    pkg_simulator = get_package_share_directory("robot_gazebo_sim")

    selected_world = "test_city"
    world_path = os.path.join(
        pkg_simulator, "resource", "worlds", f"{selected_world}.world"
    )

    gazebo_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_simulator, "launch", "gazebo.launch.py")
        ),
        launch_arguments={
            "world_path": world_path,
        }.items(),
    )

    spawn_robots_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_simulator, "launch", "spawn_robots.launch.py")
        ),
    )

    

    pkg_share = get_package_share_directory("robot_description")
    default_rviz_config_path = os.path.join(pkg_share, "rviz/urdf_config.rviz")


    
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", default_rviz_config_path],
        parameters=[{"use_sim_time": True}],
    )

    ld = LaunchDescription()

    ld.add_action(gazebo_launch)
    ld.add_action(spawn_robots_launch)
    ld.add_action(rviz_node)

    return ld
